#pragma once

#include <MeshPlaneIntersect.hpp>
#include <Eigen/Dense>
//#include <constant/label.h>
//#include <math/common.hpp>
#include <WallPlane.hpp>

class LineExtractor
{
    using Intersector = MeshPlaneIntersect<double, int>;
    template<typename T>
    using vector = std::vector<T>;
    using Vector3d = Eigen::Vector3d;
    using LineInPCL = vector<pcl::PointXYZI>;

    #if 0
    class FrameworkPlane
    {
    public:
        //Intersector::Plane ins_pl;
        Points boundary;
        Plane pl;
        int label;
        bool is_processed = false;

        void updateBoundary(const Points& new_boundary)
        {
            boundary = new_boundary;
        }

        void updateBoundary(const Point_3& p1, const Point_3& p2)
        {
            int min_idx = getClosestIdx(p1);
            int min_idx2 = getClosestIdx(p2); 
            
            boundary[min_idx] = p1;
            boundary[min_idx2] = p2;
        }

    private:
        int getClosestIdx(const Point_3& p)
        {
            int min_idx, i;
            float min_dis = 10000.0, d;
            for(i=0; i<boundary.size(); ++i)
            {
                if((d = getDistance(boundary[i], p) ) < min_dis)
                {
                    min_idx = i; 
                    min_dis = d;
                }
            }
            return i;
        }

        float getDistance(const Point_3 &p1, const Point_3 &p2)
        {
            return sqrt(  pow(p1.x()-p2.x(), 2.0) 
                        + pow(p1.y()-p2.y(), 2.0)
                        + pow(p1.z()-p2.z(), 2.0));
        }
    };
    #endif

private:
    PlaneParas* ceiling_plane;
    PlaneParas* floor_plane;
    vector<WallPlane> wall_planes;
    WallPlane* floor_plane_rect;
    WallPlane* ceiling_plane_rect;
    string out_dir;

public:

    LineExtractor(){}

    //void setOrigin(const Vector3d& value) { origin = value;}
    void setOutDir(string value){ out_dir = value;}

    void setFloorPlane(const Plane& p){floor_plane = new PlaneParas(p); floor_plane_rect = new WallPlane(p);}
    void setCeilingPlane(const Plane& p){ceiling_plane = new PlaneParas(p); ceiling_plane_rect = new WallPlane(p);}
    
    // 输入为单帧中提取到的walls
    // 然后和已固定的平面相交，更新线框
    void processWallPlanes(const vector<Plane>& walls)
    {
        for(int i=0; i<walls.size(); ++i)
            addPlane(walls[i], i);
    }

    const int getWallNum() const { return wall_planes.size();}

/*
    void initialize(const Plane& ceiling, const Plane& floor, const vector<Plane>& walls)
    {
        std::cout << "initializing ceilg and floor..." << std::endl;
        setCeilingPlane(ceiling);
        setFloorPlane(floor);

        for(auto &pl : walls)
            addPlane(pl);

        for(auto &w : wall_planes)
            w.updateFramework();
    }
*/


    // 获得最近的一条边
    Edge getClosestEdge(const Points& vertices, const PlaneParas& plane_to_close)
    {
        float min_dis = 10000.0;
        int min_idx, min_idx2;
        float d;
        for(int i=0; i<vertices.size(); ++i)
        {
            Vector3d p(vertices[i].x(), vertices[i].y(), vertices[i].z());
            if((d = plane_to_close.getDistanceFromPoint(p)) < min_dis)
            {
                min_idx = i;
                min_dis = d;
            }
        }

        min_dis = 10000.0;
        for(int i=0; i<vertices.size(); ++i)
        {
            Vector3d p(vertices[i].x(), vertices[i].y(), vertices[i].z());
            if(i != min_idx 
                && (d = plane_to_close.getDistanceFromPoint(p)) < min_dis)
            {
                min_idx2 = i;
                min_dis = d;
            }
        }

        // 找到了离平面最近的两个点的编号为min_idx和min_idx2
        return std::make_pair(min_idx, min_idx2);
    }
    
    bool isNotParallel(const PlaneParas& p1, const PlaneParas& p2)
    {
        float angle = get_angle(p1.normal, p2.normal);
        return angle > 10.0 && angle < 170.0;
    }

    // 核心函数，计算框架的算法
    void calculate(WallPlane& new_wall_plane)
    {
        Edge close_edge; 
        Points expand_wall;
        Intersector::Path3D intersect_result;

        // 和地面的交线 
        close_edge = getClosestEdge(new_wall_plane.vertices, *floor_plane);
        expand_wall = CGAL_Utils::getExpandVertices(new_wall_plane.vertices, close_edge);
        if(Intersect(expand_wall, *floor_plane, intersect_result))
        {
            updateVertices(expand_wall, close_edge, intersect_result, new_wall_plane.vertices);
            new_wall_plane.floor_edge = close_edge;
        }
        

        // 和天花板的交线
        close_edge = getClosestEdge(new_wall_plane.vertices, *ceiling_plane);
        expand_wall = CGAL_Utils::getExpandVertices(new_wall_plane.vertices, close_edge);
        if(Intersect(expand_wall, *ceiling_plane, intersect_result))
        {
            updateVertices(expand_wall, close_edge, intersect_result, new_wall_plane.vertices);
            new_wall_plane.ceiling_edge = close_edge;
        }
        

        // 和其他墙面的交线
        for(auto &w : wall_planes) // 先找候选面
        {
            if(isNotParallel(new_wall_plane.para, w.para))
            {
                
                // 找墙面w_n 距离平面w 最近的两个点
                close_edge = getClosestEdge(new_wall_plane.vertices, w.para);
                // 扩展墙面
                expand_wall = CGAL_Utils::getExpandVertices(new_wall_plane.vertices, close_edge);
                

                // if(new_wall_plane.id == 4 && w.id == 3)
                // {
                //     writePlane(new_wall_plane.vertices, out_dir+"/wall_original.obj");
                //     writePlane(expand_wall, out_dir+"/wall_expand.obj");
                //     writePlane(w.vertices, out_dir+"/wall_in_map.obj");

                //     std::cout << "is cross:" << isCross(expand_wall, w.para) << std::endl;
                // }

                // 扩展后的墙面被平面穿过，那么反过来也判断一次
                if(isCross(expand_wall, w.para))
                {
                    Edge close_edge2 = getClosestEdge(w.vertices, new_wall_plane.para);
                    Points expand_wall2 = CGAL_Utils::getExpandVertices(w.vertices, close_edge2);
                    
                    // 如果它们互相满足扩展后相交，则能够形成交线
                    if(isCross(expand_wall2, new_wall_plane.para))
                    {
                        // 调用计算交线的函数，结果存入intersect_result
                        if(Intersect(expand_wall, w.para, intersect_result))
                        {
                            // 由于边存的是索引，所以只需要更新墙面的顶点
                            updateVertices(expand_wall, close_edge, intersect_result, new_wall_plane.vertices);
                            
                            // 双向更新
                            updateVertices(expand_wall2, close_edge2, intersect_result, w.vertices);
                            
                            // 保存框线
                            new_wall_plane.wall_edges.push_back(close_edge);
                            
                            // 如果已经有两条交线了，就不用再继续找了
                            if(new_wall_plane.wall_edges.size() == 2)
                                break;
                        }
                    }
                }
            }
        }

        new_wall_plane.updateFramework();
    }

    void addPlane(const Plane& p, int plane_id)
    {
        //std::cout << "Adding plane: " << p.getLabel() << std::endl;
        WallPlane new_wall(p);
        new_wall.setId(plane_id);
        //std::cout << "Added." << std::endl;
        calculate(new_wall); // 提取框线
        wall_planes.push_back(new_wall);
    }

    /*
    // 不添加，而是更新框架
    void updatePlane(const Plane& p, int index)
    {
        PointType min_point_aabb = p.getAABB().first;
        PointType max_point_aabb = p.getAABB().second;
        Points points(wall_planes[index].vertices);
        //points.insert(wall_planes[index].vertices.begin(), 4);
        points.push_back(Point_3(min_point_aabb.x, min_point_aabb.y, min_point_aabb.z));
        points.push_back(Point_3(max_point_aabb.x, max_point_aabb.y, max_point_aabb.z));
        Plane_3 pl(p.getA(), p.getB(), p.getC(), -p.getD());
        
        // 更新增加平面点后的顶点
        wall_planes[index].vertices = CGAL_Utils::getVertices(points, pl);
        // 重新计算框架
        calculate(wall_planes[index]);
    }
    */

    // 用距离edge的端点最近的path上的点，来更新edge的端点
    void updateVertices(const Points& vertices_for_query, Edge& edge, 
        const Intersector::Path3D& path_for_query, Points& vertices_to_update)
    {
        float min_dis = 10000.0;
        Point_3 point_for_update;

        for(auto &p : path_for_query.points)
        {
            Point_3 point_for_query = Point_3(p[0], p[1], p[2]);
            float d = CGAL_Utils::getDistance(vertices_for_query[edge.first], 
                                              point_for_query);
            if(d < min_dis)
            {
                min_dis = d;
                point_for_update = point_for_query; // 距离最近的点用于更新vertices
            }
        }
        vertices_to_update[edge.first] = point_for_update;

        min_dis = 10000.0;
        for(auto &p : path_for_query.points)
        {
            Point_3 point_for_query = Point_3(p[0], p[1], p[2]);
            float d = CGAL_Utils::getDistance(vertices_for_query[edge.second], 
                                              point_for_query);
            if(d < min_dis)
            {
                min_dis = d;
                point_for_update = point_for_query; // 距离最近的点用于更新vertices
            }
        }
        vertices_to_update[edge.second] = point_for_update;
    }

    // 核心函数，不改动其接口
    bool Intersect(const Points& vertices, const PlaneParas& paras, 
                   Intersector::Path3D& result)
    {	
        using Vertex = Intersector::Vec3D;
        using Face = Intersector::Face;
        
        vector<Vertex> _vertices(4, Vertex());
        for(int i=0; i<4; ++i)
            _vertices[i] = (Vertex{vertices[i].x(), vertices[i].y(), vertices[i].z()});

        // 矩形按对角线分割成两个三角形
        vector<Face> _faces{ {0,1,2}, {0,3,2} };

        // 构造曲面用于计算
        Intersector::Mesh mesh(_vertices, _faces);

        // 平面数据转换
        Intersector::Plane p;
        p.normal = {paras.A, paras.B, paras.C};
        p.origin = {paras.center(0), paras.center(1), paras.center(2)};
        
        // 核心函数
        auto res = mesh.Intersect(p);

        // 无交线，是有可能的
        if(res.size() == 0) return false;

        result = res[0];
        return true;
    }


    // void getWallFramework()
    // {
    //     for(int i=0; i<wall_plane.size(); ++i) // 要处理的墙面
    //     {
    //         for(int j=0; j<wall_plane.size(); ++j)
    //         {
    //             if(i != j && isNotParallel(wall_plane[i], wall_plane[j])
    //                 && !wall_plane[i].is_processed)
    //             {
    //                 Rectangle rect;
                    
    //                 // 计算rect1距离plane2最近的两个点
    //                 ClosestEdge edge1 = getClosestEdge(wall_plane[i].boundary, wall_plane[j].pl);

    //                 // 假如rect1被plane2穿过
    //                 if(isCross(wall_plane[i], wall_plane[j]))
    //                 {
    //                     // 计算rect2距离plane1最近的两个点
    //                     ClosestEdge edge2 = getClosestEdge(wall_plane[j].boundary, wall_plane[i].pl);

    //                     // 边缘的最近距离不能少于1.0米
    //                     if( getClosestBoundaryDistance(wall_plane[i].boundary, wall_plane[j].boundary,
    //                             edge1, edge2) > 1.0 )
    //                         continue;

    //                     rect = CGAL_Utils::getRect(wall_plane[i].boundary);
    //                 }
    //                 else // 尝试着延伸一点
    //                 {
    //                     rect = getCloserRect(wall_plane[i].boundary, edge1);
    //                 }
                    
    //                 // 如果延伸后相交
    //                 Intersector::Path3D inter_res;
    //                 if(Intersect(rect, wall_plane[j].pl, inter_res))
    //                 {
    //                     framework.push_back(toLineInPCL(inter_res, wall_plane[j].label));
                        
    //                     auto& pts = inter_res.points;
    //                     int n = pts.size();
    //                     // std::cout << "inter_res size: " << n << std::endl;
                        
    //                     Point_3 p1(pts[0][0], pts[0][1], pts[0][2]);
    //                     Point_3 p2(pts[1][0], pts[1][1], pts[1][2]);

    //                     // 把这个墙面的边界扩大到交线处，为计算地面和天花板的交线做准备
    //                     //wall_plane[i].updateBoundary(p1, p2);
    //                     //wall_plane[i].updateBoundary(CGAL_Utils::Rectangle_to_Points(rect));
    //                 }
    //             }
    //         }
    //     }
    // }

    // // 用 plane(天花板和地面) 去交 rects(墙面)
    // void getFramework(vector<FrameworkPlane> planes, vector<FrameworkPlane> rects)
    // {
    //     for(auto f : planes) // 取得地面或天花板
    //     {
    //         writeRectangeToObjFile(CGAL_Utils::getRect(f.boundary), out_dir+"/"+label_name[f.label]+".obj");
    //         for(auto w : rects) // 取得墙面
    //         {
    //             bool is_cross = isCross(f, w);
    //             if(is_cross) // 它们相交，用地面所在平面去ins墙面
    //             {
    //                 // 原墙面
    //                 Rectangle rect = CGAL_Utils::getRect(w.boundary);
    //                 writeRectangeToObjFile(rect, out_dir+"/"+ label_name[w.label] +"_" + std::to_string(cnt++)+".obj");

    //                 // 想办法往地面延伸一部分
    //                 // 解决思路是找离地面最近的两个点，延伸它们
    //                 ClosestEdge edge1 = getClosestEdge(w.boundary, f.pl);
    //                 Rectangle rect_closer =  getCloserRect(w.boundary, edge1);
    //                 writeRectangeToObjFile(rect_closer, out_dir+"/"+ label_name[w.label]+"_"+std::to_string(cnt-1)+".obj");

    //                 Intersector::Path3D inter_res;
    //                 if(Intersect(rect_closer, f.pl, inter_res))
    //                 {
    //                     // 存下相交的部分,导入cloudcompare检查
    //                     writePath3DToObjFile(inter_res, out_dir+"/path_"+std::to_string(cnt-1)+".obj");
    //                     // 转为pclLine用pcl可视化
    //                     framework.push_back(toLineInPCL(inter_res, f.label));
    //                 }
    //             }
    //         }
    //     }
    // }

    const Framework& getFramework(int i){ return wall_planes[i].getFramework(); }

    LineInPCL toLineInPCL(const Intersector::Path3D &path3D, int label)
    {
        LineInPCL line;
        pcl::PointXYZI pt;
        for(auto &p : path3D.points)
        {
            pt.x = p.at(0);
            pt.y = p.at(1);
            pt.z = p.at(2);
            pt.intensity = label;
            line.push_back(pt);
        }
        return line;
    }    

    void writeLine(const Points& points, const Edge &edge, string path)
    {
        ofstream out(path);
        out << "v " << points[ edge.first ].x() << " " << points[ edge.first ].y() << " " << points[ edge.first ].z() << endl;
        out << "v " << points[ edge.second ].x() << " " << points[ edge.second ].y() << " " << points[ edge.second ].z() << endl;
        
        out << "l 1 2" << endl;
        out.close();
    }

    void writePlane(const Points& points, string path)
    {
        ofstream out(path);
        assert(points.size() == 4);
        for(int i=0; i<4; ++i)
            out << "v " << points[i].x() << " " << points[i].y() << " " << points[i].z() << endl;
        
        out << "f 1 2 3 4" << endl;
        out.close();
    }

    void writeRectangeToObjFile(Rectangle rect, string path)
    {
        ofstream out(path);
        for(int i=0; i<12; i += 3)
            out << "v " << rect[i] << " " << rect[i+1] << " " << rect[i+2] <<endl;
        
        out << "f 1 2 3 4" << endl;
        out.close();
    }

    void writePath3DToObjFile(Intersector::Path3D path3d, string path)
    {
        ofstream out(path);
        int i=0;
        for(auto p : path3d.points)
        {
            out << "v " << p.at(0) << " " << p.at(1) << " " << p.at(2) << endl;
            i++;
        }
        for(int j=1; j<i; ++j)
            out << "l " << j << " " << j+1 << endl;
        out.close(); 
    }

    
    // 墙面与墙面相交
    // void addWallRect(const Plane& wall_plane)
    // {
    //     Points wall_rect = getBoundary(wall_plane);
    //     Rectangle wall_rect_btm = CGAL_Utils::getRectExpandBottom(wall_rect);

    //     // 将墙体向两边延伸，方便算交线
    //     Rectangle wall_rect_side = CGAL_Utils::getRectExpandSide(wall_rect);
    //     rects.push_back(wall_rect_side);

    //     // 与地面相交
    //     auto intersects = Intersect(wall_rect_btm, floor_plane);
    //     // writePath3DToObjFile(intersects[0], output+"/wall_floor.obj");
    // }

    // void updateFloorRect(const Plane& floor_plane)
    // {
    //     floor_rect = getRect(floor_plane);
    // }

    // static Rectangle getRect(const Plane& plane)
    // {
    //     CGAL_Utils::getRect(getBoundary(plane));
    // }

    // void Plane_to_FrameworkPlane(const Plane& plane, FrameworkPlane &fpl)
    // {
    //     fpl.boundary = getBoundary(plane);
    //     //fpl.ins_pl =  Plane_to_InsPlane(plane, origin);
    //     fpl.pl = plane;
    //     fpl.label = plane.getLabel();
    // }

private:
    // 默认地面只有一个
    //Rectangle floor_rect;
    // 墙面很多个
    //vector<Rectangle> wall_rects;

    // 获得四个边界顶点
    // static Points getBoundary(const Plane &p)
    // {
    //     CGAL_Utils cgal;	
    //     Points points = cgal.Point3Ds_to_Points(p.getPoints());
    //     Plane_3 plane_3 = cgal.Plane_to_Plane_3(p);
    //     return cgal.getIntersectPoints(points, plane_3);
    // }


    bool isPositive(Point_3 pt, PlaneParas pl)
    {
        return pl.A * pt.x() + pl.B * pt.y() + pl.C * pt.z() + pl.D > 0;
    }

    // 用p1的四个顶点是否存在于p2的两侧
    // 来判断两个平面是否交叉
    bool isCross(const Points& p1, const PlaneParas& p2)
    {
        if(isPositive(p1[0], p2))
        {
            for(int i=1; i<4; ++i)
                if(!isPositive(p1[i], p2))
                    return true;
        }
        else
        {
            for(int i=1; i<4; ++i)
                if(isPositive(p1[i], p2))
                    return true;
        }

        return false;    
    }
};